#include <cv_bridge/cv_bridge.h>
#include <glog/logging.h>
#include <image_transport/image_transport.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <signal.h>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <thread>
#include "commons/rabbit_consts.h"

using namespace cv;

void MySigintHandler(int sig) {
  ROS_INFO("shutting down!");
  ros::shutdown();
  exit(0);
}
void videoFuse(Mat &big,Mat &small)
{
   Rect roi = Rect(0,0, small.cols, small.rows);
   Mat subView = big(roi); 
   small.copyTo(subView);
}
int main(int argc, char** argv) {

  // init ros
  ros::init(argc, argv, "video_fuse_node");
  ros::Time::init();
  ros::NodeHandle nh;
  signal(SIGINT, MySigintHandler);
  DLOG(INFO) << "Init Ros";
  // create filter callback
  auto callback_fn = [&](const sensor_msgs::ImageConstPtr& before,
                         const sensor_msgs::ImageConstPtr& after) {
    cv::Mat frame_before = cv_bridge::toCvShare(after, "bgr8")->image;                   
    cv::Mat frame_after = cv_bridge::toCvShare(before, "bgr8")->image;
    videoFuse(frame_before,frame_after);  //small to big
    cv::imshow("view_fuse", frame_before);                
    cv::waitKey(1);
    frame_before.release();
    frame_after.release();
    

  };
  // read data from camera node and model prediction node
  message_filters::Subscriber<sensor_msgs::Image> before_sub(
      nh, TOPIC_CAMERA, 1);
  message_filters::Subscriber<sensor_msgs::Image> after_sub(
      nh, TOPIC_CAMERA_TWO, 1);
  typedef message_filters::sync_policies::ApproximateTime<
      sensor_msgs::Image, sensor_msgs::Image>
      MySyncPolicy;
  message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(30), before_sub,
                                                   after_sub);
  sync.registerCallback(boost::bind<void>(callback_fn, _1, _2));
  ros::spin();
  return 0;
}
